#include <iostream>
#include <utility>
#include <cmath>
#include <vector>
#include <iterator>
#include <algorithm>

#include "segmentrasterization.h"
#include "cell.h"
#include "line.h"



cell_idx_t cell_idx(point_t const & point, double cell_size);
std::vector<cell_idx_t> cell_idxs(point_t const & begin, point_t const & end);
point_t cell_center(cell_idx_t const & idx, double cell_size);
bool intersects_cell(point_t const & center, segment_t const & segment, double cell_size);


std::vector<cell_t> rasterize(segment_t const & segment, double cell_size, boost::function<void (cell_t &, segment_t const &)> processor)
{
    std::vector<cell_t> res;

    std::vector<cell_idx_t> cells;
    cell_idx_t begin_idx = cell_idx(segment.first, cell_size);
    cell_idx_t end_idx = cell_idx(segment.second, cell_size);

    if(begin_idx == end_idx)
        cells.push_back(begin_idx);
    else
    {
        int min_i = std::min(begin_idx.first, end_idx.first);
        int max_i = std::max(begin_idx.first, end_idx.first);
        int min_j = std::min(begin_idx.second, end_idx.second);
        int max_j = std::max(begin_idx.second, end_idx.second);

        for(int i = min_i; i <= max_i; ++i)
        {
            for(int j = min_j; j <= max_j; ++j)
            {
                cell_idx_t idx(i, j);
                if(intersects_cell(cell_center(idx, cell_size), segment, cell_size))
                {
                    cells.push_back(idx);
                }
            }
        }
    }

    for(size_t i = 0; i < cells.size(); ++i)
    {
        cell_t new_cell(cell_center(cells[i], cell_size));
        processor(new_cell, segment);
        res.push_back(new_cell);
    }

    return res;
}

cell_idx_t cell_idx(point_t const & point, double cell_size)
{
    int idx_i = (point.x + cell_size/2.0) / (cell_size);
    if(idx_i < 0) --idx_i;
    int idx_j = (point.y + cell_size/2.0) / (cell_size);
    if(idx_j < 0) --idx_j;
    return cell_idx_t(idx_i, idx_j);
}

point_t cell_center(cell_idx_t const & idx, double cell_size)
{
    double x = idx.first * cell_size;
    double y = idx.second * cell_size;
    return point_t(x, y);
}

bool intersects_cell(point_t const & center, segment_t const & segment, double cell_size)
{
    double radius = sqrt(cell_size * cell_size / 2);
    line_t line(segment.first, segment.second);
    double d = abs(distance(line, center));

    if(d < (cell_size / 2))
        return true;
    if (d < radius)
    {
        std::vector<point_t> corners;
        corners.push_back(point_t(center.x - cell_size/2, center.y - cell_size/2));
        corners.push_back(point_t(center.x - cell_size/2, center.y + cell_size/2));
        corners.push_back(point_t(center.x + cell_size/2, center.y + cell_size/2));
        corners.push_back(point_t(center.x + cell_size/2, center.y - cell_size/2));

        std::vector<point_t>::iterator i = corners.begin();
        for(; i != corners.end() - 1; ++i)
        {
            segment_t border(*i, *(i + 1));
            if(intersects(border, segment))
                return true;
        }
        segment_t border(*corners.begin(), *(corners.end() - 1));
        if(intersects(border, segment))
                return true;
    }
    return false;
}
